
#ifndef GYRO_FLOW_PORT_BASICTYPE_H
#define GYRO_FLOW_PORT_BASICTYPE_H

#include <stdio.h>
#include <math.h>
#include <string.h>

# define PI           3.1415927f


#define CD_INTER_LINEAR 0
#define CD_INTER_NEAREST 1
#define CD_BORDER_CONSTANT 0
#define CD_WARP_INVERSE_MAP 1
#define CD_CHANNEL_Y 0
#define CD_CHANNEL_UV 1



#define CD_INTER_TAB_SIZE 32
#define CD_INTER_BITS 5
#define CD_INTER_AREA 3
#define CD_INTER_MAX 7
#define CD_INT_MIN (-2147483647 - 1)
#define CD_INT_MAX 2147483647

typedef struct {
    int width;
    int height;
    unsigned char* data;
} ImageCD;

typedef struct {
    int rows;
    int cols;
    double* data;
} MatrixCD;

typedef struct {
    double val[4];
} ScalarCD;

typedef struct {
    int start;
    int end;
} RangeCD;






typedef struct Matrix3 {
    float data[3][3];
};

typedef struct Quaternion {
    // w, x, y, z
    float q0, q1, q2, q3;
} Quaternion;

typedef struct Vector3{
    float x, y, z;
} Vector3;

typedef struct Pose3 {
    float pitch, yaw, roll;
} Pose3;


Vector3* initVector3(float x, float y, float z);

Quaternion* initQuaternion(float q0, float q1, float q2, float q3);

void setQuaternion(Quaternion *q, float q0, float q1, float q2, float q3);

float length(Quaternion *q);

void normalize(Quaternion *q);

void inverse(Quaternion *q);

// a * b
void multiply(Quaternion *a, Quaternion *b, Quaternion *result);

// calculate rqr'
void rotate(Quaternion *r, Quaternion *q, Quaternion *result);

void setFromAngleAxis(Quaternion *q, float angle, float vx, float vy, float vz);

// linear slerp
void slerp(Quaternion *q1, Quaternion *q2, float alpha, Quaternion *result);

//calculate rotation quaternion from q1 -> q2
void calculateRotationQuaternion(Quaternion *q1, Quaternion *q2, Quaternion *result);


Matrix3 toRotationMatrix(Quaternion* q);

Matrix3 matrix_transpose(Matrix3* mat);

Matrix3 matrix_adjoint(Matrix3* mat);

Matrix3 matrix_inverse(Matrix3* mat);

void matrix_multiply(Matrix3* result, const Matrix3* a, const Matrix3* b);

Matrix3 K_inverse(Matrix3* mat);

void initUnitCircle(Matrix3 *matrix);

void initK(Matrix3 *matrix);

void initC2I(Matrix3 *matrix);

void invert(const MatrixCD* src, MatrixCD* dst);

int warpPerspectiveCD(const ImageCD* src, ImageCD* dst, const MatrixCD* M, int flags, int borderMode, const ScalarCD* borderValue);

int warpPerspectiveQuarter(const unsigned char* src, unsigned char* dst,
                       const unsigned char* srcU, unsigned char* dstU,
                       const unsigned char* srcV, unsigned char* dstV,
                       int start, int end, int wight, int height,
                       const MatrixCD* M, int flags, int borderMode, const ScalarCD* borderValue);

int warpPerspectiveUV(const ImageCD* srcY, ImageCD* dstY,
                      const ImageCD* srcU, ImageCD* dstU,
                      const ImageCD* srcV, ImageCD* dstV,
                      const MatrixCD* M, int flags, int borderMode, const ScalarCD* borderValue);


int warpPerspectiveFixedPoint(const ImageCD* src, ImageCD* dst, const MatrixCD* M, int flags, int borderMode, const ScalarCD* borderValue);

#endif //GYRO_FLOW_PORT_BASICTYPE_H
